#include <My_INC/PHY/PHY_Filter.h>
#include <My_INC/PHY/PHY_IIRFilter.h>

/*==================================滑动平均滤波器算法===================================*/
void Filter_MovAvg_Init(FilterMovAvg_Typedef *Record, float Freq, float ts)
{
    //    Record->Multiplier = ((6.28318530f * Freq * ts)); // 滤波器系数
    //    Record->Yn_1 = 0;                                 // 初始化 Yn-1
}
/*==================================低通滤波器算法===================================*/

/**
 * @brief 初始化低通滤波器
 * @param Record 存储滤波器状态的结构体指针
 * @param Freq 目标截止频率，单位Hz
 * @param ts 执行周期，单位秒
 */
void Filter_Low_Init(FilterLow_Typedef *Record, float Freq, float ts)
{
    float Wf = 6.2831853071795f * Freq;
    float temp = Wf * ts;
    Record->Coeff_B0 = (temp) / ((temp) + 2.0f);
    Record->Coeff_A1 = ((temp)-2.0f) / (temp + 2.0f);

    Record->Xn_1 = 0.0f;
    Record->Yn_1 = 0.0f;
}
/**
 * @brief 初始化梯形滤波器
 * @param Record 存储滤波器状态的结构体指针
 * @param wo 角频率，单位rad/s
 * @param ws 阻尼系数
 * @param ts 执行周期，单位秒
 */
void Filter_Trap_Init(FilterTrap_Typedef *filterTrap, float ts, float wn, float c2, float c1)
{
    if (filterTrap == NULL)
        return;
    // 根据传递函数计算系数（离散化公式）
    float x = (float)(2.0f * c2 * wn * ts); // 分子一阶项系数
    float y = (float)(2.0f * c1 * wn * ts); // 分母一阶项系数
    float z = (float)(wn * ts * wn * ts);   // 分母二阶项系数

    float A1 = y - 2.0f;
    float A2 = z - y + 1.0f;
    float B0 = (1.0f);
    float B1 = (x - 2);
    float B2 = (z - x + 1.0f);
    Coeff2P2Z_Init(&filterTrap->notch, B0, B1, B2, A1, A2);
}
void Filter_Trap_Reset(FilterTrap_Typedef *filterTrap)
{
    if (filterTrap == NULL)
        return;
    Coeff2P2Z_Reset(&filterTrap->notch);
}